#include "OpenDRIVE.h"
#include <iostream>
#include <fstream>
#include <queue>
#include "../tools/odrSpiral.h"
#include "../../constants/MathConstants.h"
#include "../../constants/DecisionConstants.h"
#include "../../constants/CarConstants.h"
#include "CubicLine.h"
namespace OpenDrive {
	OpenDrive::OpenDrive()
	{
		vaildState = false;
	}
	OpenDrive::OpenDrive(std::string odrvfileName)
	{
		open(odrvfileName);
	}

	OpenDrive::~OpenDrive()
	{
	}

	void OpenDrive::addRaod(const Road &road)
	{
		roads.insert(std::pair<std::string, Road>(road.getId(), road));
	}

	void OpenDrive::addJunction(const Junction &junction)
	{
		junctions.insert(std::pair<std::string, Junction>(junction.getId(), junction));
	}
	std::string OpenDrive::getLaneType(std::string roadId, int laneSectionId, int laneId)
	{
		return getLane(roadId, laneSectionId, laneId).getType();
	}
	bool OpenDrive::roadExist(std::string roadId)
	{
		return roads.find(roadId) != roads.end();
	}
	std::string OpenDrive::getLaneType(int uniqueId)
	{
		return getLaneType(nodes[uniqueId].roadId, nodes[uniqueId].laneSectionId, nodes[uniqueId].laneId);
	}
	float OpenDrive::getLaneLength(std::string roadId, int laneSectionId, int laneId)
	{

		Lane &lane = getLane(roadId, laneSectionId, laneId);
		float length = lane.getLength();
		float startS;
		float endS;
		if (length > 0) {
			return length;
		}
		Road& road = getRoad(roadId);
		Lanes& lanes = road.getLanes();
		startS = lanes.getLaneSectionById(laneSectionId).getS();
		if (laneSectionId == lanes.getLaneSections().size() - 1) {
			endS = road.getLength();
		}
		else {
			endS = lanes.getLaneSectionById(laneSectionId + 1).getS();
		}
		length = endS - startS;
		lane.setLength(length);
		return length;
	}
	float OpenDrive::getLaneLength(int uniqueId)
	{
		Node node = nodes[uniqueId];
		return getLaneLength(node.roadId, node.laneSectionId, node.laneId);
	}
	bool OpenDrive::laneExist(std::string roadId,int laneSectionId, int laneId)
	{
		return roadExist(roadId) && roads[roadId].laneExist(laneSectionId, laneId);
	}
	Lane& OpenDrive::getLane(std::string roadId, int laneSectionId, int laneId)
	{
		/*if (!roadExist(roadId))
			return nullptr;
		LaneSection& laneSection = getRoad(roadId)->getLanes()->getLaneSectionById(laneSectionId);
		if (laneSection == nullptr)
			return nullptr;
		return laneSection->getLaneByLaneId(laneId);*/
		return roads[roadId].getLane(laneSectionId, laneId);

	}
	Lane & OpenDrive::getLane(int uniqueId)
	{
		Node node = nodes[uniqueId];
		return getLane(node.roadId, node.laneSectionId, node.laneId);
	}
	int OpenDrive::getLaneUniqueId(std::string roadId, int laneSectionId, int laneId)
	{
		if (false == laneExist(roadId,laneSectionId,laneId)) {
			return -1;
		}
		Lane &lane = getLane(roadId, laneSectionId, laneId);
		return lane.getUniqueId();
	}
	bool OpenDrive::valid()
	{
		return vaildState;
	}
	void OpenDrive::clear()
	{
		nodes.clear();
		roads.clear();
		junctions.clear();
		invalidPosition.clear();
		if (doc != nullptr) {
			delete doc;
			doc = nullptr;
		}
		laneNum = 0;
		vaildState = false;
	}
	bool OpenDrive::open(std::string odrvfileName)
	{
		clear();
		prefix_output_path = R"(files/output/)";
		doc = new TiXmlDocument();
		stepLength = 0.5;
		Z_ = -2;
		laneNum = 0;
		vaildState = true;
		debug = true;
		//如果读取失败,将tinyxml对象赋值为nullptr
		if (!doc->LoadFile(odrvfileName.c_str())) {
			std::cerr << "读取文件失败:" << doc->ErrorDesc() << std::endl;
			delete doc;
			doc = nullptr;
			vaildState = false;
			return false;
		}
		if (debug)
			std::cout << "正在解析文件..." << std::endl;
		vaildState = parseOpenDriveFile();
		if (vaildState) {
			if (debug) {
				std::cout << "解析成功" << std::endl;
				std::cout << "正在构建连接关系.." << std::endl;
			}

			vaildState = buildMap();
			if (vaildState) {
				if (debug) {
					std::cout << "构建连接关系成功" << std::endl;
					//打印图
					std::cout << "打印连接关系.." << std::endl;
					print();
					//计算车道线、中心线
					std::cout << "计算车道线、参考线、中心线" << std::endl;
				}
				calLaneLineAndCenterLine();
				if (debug) {
					//输出车道线、中心线
					std::cout << "输出车道线、参考线、中心线" << std::endl;
					outputPointsToFile();
					std::cout << "按编号输出参考线" << std::endl;
					outputRefPointsToFileByNum();
				}
			}
			else {
				std::cout << "构建连接关系失败" << std::endl;
			}
		}
		else {
			std::cout << "解析失败" << std::endl;
		}
		return vaildState;
	}
	//AstarPlan
	bool OpenDrive::AstarPlan(Position start, Position end, bool append, bool turnRound)
	{
		//float INFIDIS = 1000000;
		struct Vertax {
			//道路的唯一id(uniqueId)
			int id;
			float cost;
			int parent;
			int closed;
			//flag==1表示前车道经过换道到达此车道（同向），flag==2表示前车道经过掉头到达此车道，会越过参考线，前车道与当前车道反向
			int flag;
			////offset 沿着车道的行驶方向从0-length-1
			float startOffset;
		};
		struct cmp {
			bool operator()(struct Vertax *&a, struct Vertax *&b)const {
				return a->cost > b->cost;
			}
		};
		std::priority_queue<Vertax*, std::vector<Vertax*>, cmp> open;

		struct Vertax *vertaxs = new struct Vertax[laneNum];
		for (int i = 0; i < laneNum; i++) {
			vertaxs[i].id = i;
			vertaxs[i].parent = -1;
			vertaxs[i].closed = 0;
			//INFDIS表示无穷大，此路不通
			vertaxs[i].cost = INFDIS;
		}
		Lane* startLane = &getLane(start.roadId, start.laneSectionId, start.laneId);
		Lane* endLane = &getLane(end.roadId, end.laneSectionId, end.laneId);
		//换道的三次样条索引相差的值
		unsigned int laneChangeIndexInterval = 40;
		if (startLane == nullptr || endLane == nullptr) {
			std::cout << "起始路径不存在" << std::endl;
			return false;
		}
		//特殊情况：当起点和终点在同一条道路节点时，并且终点在前起点在后，此时只能转一圈到达终点,需要两次将道路节点从open表中移除
		//每从open表中移除一次目的节点，remainRemovalTimes减少一，如果remainRemovalTimes并且到达目标节点，说明找到路径。默认为一，特殊情况下为二
		int remainRemovalTimes = 1;
		if (startLane == endLane && start.offset > end.offset) {
			remainRemovalTimes = 2;
			//std::cout << "相同车道寻路。。"<<std::endl;
		}
		int startId = startLane->getUniqueId();
		int endId = endLane->getUniqueId();
		vertaxs[startId].cost = 0;
		vertaxs[startId].startOffset = start.offset;
		open.push(&vertaxs[startId]);
		while (!open.empty()) {
			//从open表中弹出最小路径代价的节点
			const struct Vertax *const minCostNode = open.top();
			Lane* minLane = &getLane(nodes[minCostNode->id].roadId, nodes[minCostNode->id].laneSectionId, nodes[minCostNode->id].laneId);
			//std::cout << "minCostNodeID:" << minCostNode->id << std::endl;
			open.pop();
			//如果最小代价的节点已经无穷大，则不必要搜索
			if (vertaxs[minCostNode->id].cost >= INFDIS) {
				break;
			}
			//将此节点closed置1表示从start节点到该节点的最短路径已经找到
			vertaxs[minCostNode->id].closed = 1;
			//如果到达目的节点,则需要判断
			if (minCostNode->id == endId) {
				remainRemovalTimes--;
				if (remainRemovalTimes == 0) {
					break;
				}
				else {
					vertaxs[minCostNode->id].closed = 0;
				}
			}
			//cost表示从起点到当前道路的startOffset的代价
			float weight, minCosLaneLength = getLaneLength(minCostNode->id);
			weight = minCosLaneLength - vertaxs[minCostNode->id].startOffset;
			//minCostNode节点的所有后继节点
			Edge *neiborNode = nodes[minCostNode->id].next;
			while (neiborNode) {
				//从open表中弹出最小路径代价的节点
				int neiborNodeId = neiborNode->id;
				Lane* neiborLane = &getLane(nodes[neiborNodeId].roadId, nodes[neiborNodeId].laneSectionId, nodes[neiborNodeId].laneId);
				//判断neiborLane路段是否可行:如果minLane有路障，并且沿着minLane的行驶方向会有invalidPosition(路障位置)，则neiborLane不可行
				bool neiborLaneValid = true;
				int invalidPositionSize = invalidPosition.size();
				for (int i = 0; i < invalidPositionSize; i++) {
					int invalidId = getLaneUniqueId(invalidPosition[i].roadId, invalidPosition[i].laneSectionId, invalidPosition[i].laneId);
					if (invalidId == minCostNode->id) {
						float offsetInMinLane = invalidPosition[i].offset;
						float carOffsetInMinLane = minCostNode->startOffset;
						if (carOffsetInMinLane <= offsetInMinLane) {
							neiborLaneValid = false;
						}
					}
				}
				//如果没有被路障挡住行驶方向&&该节点是driving && 该节点最短路径未找到 && 该节点的前驱节点不是minCostNode
				if (neiborLaneValid && neiborLane->getType() == "driving" && vertaxs[neiborNodeId].closed == 0 && vertaxs[neiborNodeId].parent != minCostNode->id) {
					//如果是特殊情况或代价更短
					if (vertaxs[neiborNodeId].cost == 0 || vertaxs[neiborNodeId].cost > vertaxs[minCostNode->id].cost + weight) {
						//换道标志
						vertaxs[neiborNodeId].flag = 0;
						//如果没换道则是从起点开始
						vertaxs[neiborNodeId].startOffset = 0;
						//更新代价
						vertaxs[neiborNodeId].cost = vertaxs[minCostNode->id].cost + weight;
						//如果该节点没有在open表中,放入open表
						if (vertaxs[neiborNodeId].parent == -1) {
							open.push(&vertaxs[neiborNodeId]);
						}
						//更新父节点
						vertaxs[neiborNodeId].parent = minCostNode->id;
					}
				}
				neiborNode = neiborNode->next;
			}
			///下面是换道和掉头的考虑
			int laneId = minLane->getId();
			const std::vector<int> &laneChange = minLane->getLaneChangeFlag();
			unsigned int laneChangeSize = laneChange.size();
			//s[0]表示左侧车道的车道编号，s[1]表示右侧车道的车道编号
			int s[2];
			//indexOfLane表示换道位置在当前车道点的位置下标
			unsigned int indexOfLane;
			//如果位于参考线右侧车道
			if (laneId < 0) {
				//如果该车道是最左边车道，则该车道左车道为逆向车道
				if (laneId == -1) {
					s[0] = getLaneUniqueId(nodes[minCostNode->id].roadId, nodes[minCostNode->id].laneSectionId, 1);
				}
				else {
					s[0] = getLaneUniqueId(nodes[minCostNode->id].roadId, nodes[minCostNode->id].laneSectionId, nodes[minCostNode->id].laneId + 1);
				}
				s[1] = getLaneUniqueId(nodes[minCostNode->id].roadId, nodes[minCostNode->id].laneSectionId, nodes[minCostNode->id].laneId - 1);
			}
			//如果位于参考线左侧车道
			if (laneId > 0) {
				//如果该车道是最左边车道，则该车道左车道为逆向车道
				if (laneId == 1) {
					s[0] = getLaneUniqueId(nodes[minCostNode->id].roadId, nodes[minCostNode->id].laneSectionId, -1);
				}
				else {
					s[0] = getLaneUniqueId(nodes[minCostNode->id].roadId, nodes[minCostNode->id].laneSectionId, nodes[minCostNode->id].laneId - 1);
				}
				s[1] = getLaneUniqueId(nodes[minCostNode->id].roadId, nodes[minCostNode->id].laneSectionId, nodes[minCostNode->id].laneId + 1);
			}
			if (minCostNode->id == 2184) {
				int fdgfg = 4;
			}

			indexOfLane = laneChangeSize*vertaxs[minCostNode->id].startOffset / minCosLaneLength;
			indexOfLane = std::min((unsigned int)(laneChangeSize - 1), indexOfLane);
			int laneChangeFlag = laneChange[indexOfLane];
			//定义越道代价,直行到可以换道的距离代价
			float crossLaneCost, straightCost;
			bool turnRoundFlag;
			float laneChangeOffset;
			//开始换道的索引（centerLine中)
			int laneChangeIndexStart;
			for (int i = 0; i < 2; i++) {
				crossLaneCost = 5;
				turnRoundFlag = false;
				laneChangeIndexStart = indexOfLane;
				//如果该节点编号有效 && //如果是可行使道路 && 该节点最短路径未找到 && 该节点的前驱节点不是minCostNode 
				if (s[i] != -1 && getLaneType(s[i]) == "driving" && vertaxs[s[i]].closed == 0 && vertaxs[s[i]].parent != minCostNode->id) {
					Lane& tempLane = getLane(s[i]);
					const std::vector<float> &tempLaneWidths = tempLane.getLaneWidth();
					//如果要换道相反方向的道路上(掉头)&&不允许掉头,则不掉头换道
					if ((nodes[s[i]].laneId*laneId < 0 && turnRound == false)) {
						continue;
					}
					//如果要换道相反方向的道路上(掉头)&&允许掉头,则可以掉头换道
					if ((nodes[s[i]].laneId*laneId < 0 && turnRound == true)) {
						turnRoundFlag = true;
						crossLaneCost *= 20;
					}
					//往左掉头或者换道
					if (i == 0) {
						//一直搜索到可以往左换道的标志出现 并且左道宽度不为0
						while (laneChangeIndexStart < laneChangeSize && (laneChange[laneChangeIndexStart] != dcc::LaneChangeLeft\
							&& laneChange[laneChangeIndexStart] != dcc::LaneChangeBoth || tempLaneWidths[laneChangeIndexStart] < 0.1/*carc::widthOfCar*/)) {
							laneChangeIndexStart++;
						}
					}
					//向右换道（向右应该没有掉头）
					else if (i == 1) {
						//一直搜索到可以往右换道的标志出现 并且右道宽度不为0
						while (laneChangeIndexStart < laneChangeSize && (laneChange[laneChangeIndexStart] != dcc::LaneChangeRight\
							&& laneChange[laneChangeIndexStart] != dcc::LaneChangeBoth || tempLaneWidths[laneChangeIndexStart] < 0.1/*carc::widthOfCar*/)) {
							laneChangeIndexStart++;
						}
					}
					//不允许掉头或者换道
					if (laneChangeIndexStart >= laneChangeSize) {
						continue;
					}
					//laneChangeOffset = (float)(laneChangeIndexStart) / laneChangeSize * getLaneLength(minCostNode->id);
					//vertaxs[minCostNode->id].startOffset = laneChangeOffset;
					//继续向前索引n个
					laneChangeIndexStart = std::min(laneChangeSize, laneChangeIndexStart + laneChangeIndexInterval);
					//在当前车道沿车道行驶方向的偏移,如果是换道则不变
					laneChangeOffset = (float)(laneChangeIndexStart) / laneChangeSize * minCosLaneLength;
					straightCost = laneChangeOffset - vertaxs[minCostNode->id].startOffset;
					//表示掉头
					if (nodes[s[i]].laneId*laneId < 0) {
						laneChangeOffset = minCosLaneLength - laneChangeOffset;
					}
					//如果代价更短
					if (vertaxs[s[i]].cost > vertaxs[minCostNode->id].cost + crossLaneCost + straightCost) {
						//更新代价
						vertaxs[s[i]].cost = vertaxs[minCostNode->id].cost + crossLaneCost + straightCost;
						//换道标志1,掉头标志2					
						vertaxs[s[i]].flag = turnRoundFlag ? 2 : 1;
						//如果该节点没有在open表中,放入open表
						if (vertaxs[s[i]].parent == -1) {
							open.push(&vertaxs[s[i]]);
						}
						//更新父节点
						vertaxs[s[i]].parent = minCostNode->id;
						vertaxs[s[i]].startOffset = laneChangeOffset;
					}
				}
			}
		}

		//如果未找到最短路径，则退出
		if (vertaxs[endId].closed == 0) {
			std::cout << "can't reach!" << std::endl;
			return false;
		}
		//保存搜索的路径编号，从终点到起点
		std::vector<int> uniqueLaneIdFromEndToStart;
		uniqueLaneIdFromEndToStart.push_back(endId);
		std::cout << "(" << end.roadId << "," << end.laneSectionId << "," << end.laneId << ")";
		int i = vertaxs[endId].parent;
		while (i != -1) {
			std::cout << "<--(" << nodes[vertaxs[i].id].roadId << "," << nodes[vertaxs[i].id].laneSectionId << "," << nodes[vertaxs[i].id].laneId << ")";
			uniqueLaneIdFromEndToStart.push_back(vertaxs[i].id);
			if (vertaxs[i].id == endId) {
				break;
			}
			i = vertaxs[i].parent;
		}
		std::cout << std::endl;

		//输出路径点
		int pathSize = uniqueLaneIdFromEndToStart.size();
		std::fstream path;
		std::fstream pathForPlot;
		if (append) {
			path.open(prefix_output_path+"path.txt", std::ios::app);
			pathForPlot.open(prefix_output_path+"pathForPlot.txt", std::ios::app);
		}
		else {
			path.open(prefix_output_path+"path.txt", std::ios::out);
			pathForPlot.open(prefix_output_path+"pathForPlot.txt", std::ios::out);
		}

		int total = 0;
		float preX;
		float preY;
		for (int i = pathSize - 1; i >= 0; i--) {
			bool turnRoundFlag = false;
			//bool changeLane = false;
			int laneChangeStartIndex = -1;
			int laneChangeEndIndex = -1;
			int uniqueId = uniqueLaneIdFromEndToStart[i];
			Lane* lane = &getLane(uniqueId);
			Lane* leftLane, *rightLane;
			std::string roadId = lane->getRoadId();
			int laneSectionId = lane->getLaneSectionId();
			int laneId = lane->getId();
			if (lane->getId() > 0) {
				leftLane = laneExist(roadId, laneSectionId, laneId - 1) ? &getLane(roadId, laneSectionId, laneId - 1) : nullptr;
				rightLane = laneExist(roadId, laneSectionId, laneId + 1) ? &getLane(roadId, laneSectionId, laneId + 1) : nullptr;
			}
			else {
				leftLane = laneExist(roadId, laneSectionId, laneId + 1) ? &getLane(roadId, laneSectionId, laneId + 1) : nullptr;
				rightLane = laneExist(roadId, laneSectionId, laneId - 1) ? &getLane(roadId, laneSectionId, laneId - 1) : nullptr;
			}
			float laneLength = getLaneLength(uniqueId);
			//会改变，所以不使用引用
			std::vector<Point> centerLine = lane->getCenterLine();
			const std::vector<float> &laneWidth = lane->getLaneWidth();
			const std::vector<int> &laneChangeFlag = lane->getLaneChangeFlag();
			const std::vector<short> &turnLightFlag = lane->getTurnLightFlag();
			const std::vector<short> &crossWalkFlag = lane->getCrossWalkFlag();
			std::vector<float> leftLaneWidth;
			std::vector<float> rightLaneWidth;
			if (leftLane != nullptr) {
				leftLaneWidth = leftLane->getLaneWidth();
			}
			if (rightLane != nullptr) {
				rightLaneWidth = rightLane->getLaneWidth();
			}
			unsigned int startIndex;
			unsigned int endIndex = centerLine.size();
			unsigned int centerLineSize = endIndex;
			//起点道路,必须分开讨论，因为当终点和起点在同一条lane时，startOffset会被覆盖，但是start和end的offset不会改变
			if (i == pathSize - 1) {
				startIndex = ceil(start.offset / laneLength * centerLineSize);
			}
			else {
				startIndex = ceil(vertaxs[uniqueId].startOffset / laneLength * centerLineSize);
			}
			//终点道路
			if (i == 0) {
				endIndex = end.offset / laneLength * centerLineSize;
			}//如果下一条道是由当前道换道过来的，换道只在同一个laneSection中换，因此道路的长度相同，点的个数也相同
			else if (vertaxs[uniqueLaneIdFromEndToStart[i - 1]].flag == 1) {
				//换道过来的，则需要用三次样条拟合换道轨迹
				//如果连续换道，直接找最后一条道
				int k = i - 1;
				while (k >= 0 && vertaxs[uniqueLaneIdFromEndToStart[k]].flag == 1) {
					k--;
				}
				k++;
				//连续换道，将道路编号置位，这里i改变了，注意下，下面掉头用到i了，但掉头和换道只会发生一个，因此不会影响
				i = k + 1;
				endIndex = vertaxs[uniqueLaneIdFromEndToStart[k]].startOffset / laneLength * centerLineSize;
				Point start = centerLine[std::max(0u, endIndex - laneChangeIndexInterval)];
				Point end = getLane(uniqueLaneIdFromEndToStart[k]).getCenterLine()[std::min(centerLineSize-1, endIndex)];
				CubicLine cubicLine(start, end);
				std::vector<Point>&& temp = cubicLine.calPoints();
				endIndex = std::max(0u, endIndex - laneChangeIndexInterval);
				laneChangeStartIndex = endIndex + 1;
				for (int j = 0,size=temp.size(); j < size; j++) {
					if (centerLineSize <= endIndex) {
						centerLine.push_back(temp[j]);
					}
					else {
						centerLine[endIndex++] = temp[j];
					}
				}
				
				laneChangeEndIndex = endIndex;
				//changeLane = true;

			}//如果下一条道是由当前道掉头过来的
			else if (vertaxs[uniqueLaneIdFromEndToStart[i - 1]].flag == 2) {
				endIndex = (laneLength - vertaxs[uniqueLaneIdFromEndToStart[i - 1]].startOffset) / laneLength * centerLineSize;
				turnRoundFlag = true;
			}
			float lfWidth = 0, rgWidth = 0, midWidth = 0, speed = 10.0;//m/s
			short GPS = 1, roadType = 0, stopTime = 0,sign = dcc::SeeNoLight, ifOnCrossWalk = dcc::outCrossWalk, laneChangef = dcc::LaneChangeNone;
			int Area = dcc::OutArea, turnLight = dcc::SeeNoLight;
			float intervalDis = 0.3f;
			for (int j = startIndex; j < endIndex; j++) {				
				Area = 0;
				if (endIndex - j < 10 && turnRoundFlag){
					Area = 2;
				}
				//第一个点||当前点距离前一个输出点距离不太近
				if (total == 0 || !((pow(centerLine[j].getX() - preX, 2) + pow(centerLine[j].getY() - preY, 2)) < intervalDis)) {
					GPS = 1, roadType = 0, stopTime = 0, sign = dcc::SeeNoLight, ifOnCrossWalk = dcc::outCrossWalk, laneChangef = dcc::LaneChangeNone;
					lfWidth = 0, rgWidth = 0,midWidth = 0;
					Area = dcc::OutArea, turnLight = dcc::SeeNoLight;
					turnLight = 0;
					//三次样条拟合的点给个固定宽度
					if (j >= laneChangeStartIndex && j < laneChangeEndIndex) {
						midWidth = 3.65f;
					}
					else {
						//这个j在这里面不会越界
						midWidth = laneWidth[j];
						ifOnCrossWalk = crossWalkFlag[j];
						sign = turnLightFlag[j];
						laneChangef = laneChangeFlag[j];
						if (leftLane != nullptr && leftLane->getType() == "driving") {
							lfWidth = leftLaneWidth[j];
						}
						if (rightLane != nullptr && rightLane->getType() == "driving") {
							rgWidth = rightLaneWidth[j];
						}
					}
					//如果不能往左换道，则左道宽度为0
					if (laneChangef == dcc::LaneChangeNone || laneChangef == dcc::LaneChangeRight) {
						lfWidth = 0.0;
					}//可以往左换道，但是左道为0
					else if ((laneChangef == dcc::LaneChangeBoth || laneChangef == dcc::LaneChangeLeft) && lfWidth < 0.1) {
						lfWidth = 3.5;
					}
					//如果不能往右换道，则右道宽度为0
					if (laneChangef == dcc::LaneChangeNone || laneChangef == dcc::LaneChangeLeft) {
						rgWidth = 0.0;
					}
					else if ((laneChangef == dcc::LaneChangeBoth || laneChangef == dcc::LaneChangeRight) && rgWidth < 0.1) {
						rgWidth = 3.5;
					}

					path << total << "," << centerLine[j].getX() << "," << centerLine[j].getY() << "," << speed << "," << GPS\
						<< "," << roadType << "," << ifOnCrossWalk << ","<<stopTime<<"," << sign << ","<<laneChangef << ","\
						<<Area<<"," << midWidth << "," << lfWidth << "," << rgWidth << "," << turnLight <<","<< centerLine[j].getTheta()<< std::endl;

					pathForPlot << centerLine[j].getX() << "," << centerLine[j].getY() << "," << Z_ << std::endl;
					preX = centerLine[j].getX();
					preY = centerLine[j].getY();
					total++;
				}
			}
			//掉头的时候，将路径用圆弧补上，需要半径长度，起点方向和圆弧长度
			if (turnRoundFlag) {
				speed = 10.0, GPS = 1, roadType = 0, stopTime = 0, sign = dcc::SeeNoLight, ifOnCrossWalk = dcc::outCrossWalk, laneChangef = dcc::LaneChangeNone;
				Area = dcc::InHighWayUTurn, lfWidth = 3.7f, rgWidth = 3.7f;
				float r, arcLength;
				Point sp(0, 0, 0, 0);
				////掉头一般涉及到换道，需要向前索引到没有换道的那条道，进行圆弧连接
				//int k = i;
				//while (vertaxs[uniqueLaneIdFromEndToStart[k]].flag == 1) {
				//	k++;
				//}
				Lane* startLane = &getLane(uniqueLaneIdFromEndToStart[i]);
				const std::vector<Point>& startLaneCenterLine = startLane->getCenterLine();
				if (endIndex - 1 >= 0) {
					sp = startLaneCenterLine[endIndex - 1];
				}
				else {
					sp = startLaneCenterLine[startIndex];
				}
				//同一laneSection的道路长度都一样，中心线点的个数也一样，掉头和换道都是在同一个laneSection中进行的
				Lane* nextLane = &getLane(uniqueLaneIdFromEndToStart[i - 1]);
				const std::vector<Point> &nextCenterLine = nextLane->getCenterLine();
				Point e = nextCenterLine[centerLineSize - endIndex - 1];
				r = 0.5f*sqrt(pow((sp.getX() - e.getX()), 2) + pow((sp.getY() - e.getY()), 2));
				arcLength = mtc::M_PI*r;//半圆长度
				Arc m(sp.getS(), sp, sp.getTheta(), arcLength, 1.0 / r);
				m.calPosition(stepLength);
				const std::vector<Point> &ps = m.getPoints();
				for (int j = 0, size = ps.size(); j < size; j++) {
					//第一个点||当前点距离前一个输出点距离不太近
					if (total == 0 || !((pow(ps[j].getX() - preX, 2) + pow(ps[j].getY() - preY, 2)) < intervalDis)) {
						path << total << "," << centerLine[j].getX() << "," << centerLine[j].getY() << "," << speed << "," << GPS\
							<< "," << roadType << "," << ifOnCrossWalk << "," << stopTime << "," << sign << "," << laneChangef << ","\
							<< Area << "," << midWidth << "," << lfWidth << "," << rgWidth << "," << turnLight << "," << centerLine[j].getTheta() << std::endl;
						//path << total << "," << ps[j].getX() << "," << ps[j].getY() << "," << "," << speed << "," << GPS\
						//	<< "," << roadType << "," << ifOnCrossWalk << "," << stopTime << "," << sign << "," << laneChangef << ","\;
						//path << laneChangeFlag[j] << ","<<Area<<"," << laneWidth[j] << ",3.75,3.75,0," <<ps[j].getTheta()<< std::endl;//输出为2，用非结构化道路通过
						//"," << lfWidth << "," << rgWidth<<","<<0<< std::endl;
						pathForPlot << ps[j].getX() << "," << ps[j].getY() << "," << Z_ << std::endl;
						preX = ps[j].getX();
						preY = ps[j].getY();
						total++;
					}
				}
			}
		}
		path.close();
		pathForPlot.close();

		//释放空间
		delete[] vertaxs;
		return true;
	}
	/*bool OpenDrive::AstarPlan(Position start, Position end)
	{
		//float INFIDIS = 1000000;
		struct Vertax {
			int id;
			float cost;
			int parent;
			int closed;
			//flag==1表示前车道经过换道到达此车道
			int flag;
			////offset 沿着车道线的行驶方向从0-length
			float startOffset;
		};
		struct cmp {
			bool operator()(struct Vertax *&a, struct Vertax *&b)const {
				return a->cost > b->cost;
			}
		};
		std::priority_queue<Vertax*, std::vector<Vertax*>, cmp> open;

		struct Vertax *vertaxs = new struct Vertax[laneNum];
		for (int i = 0; i < laneNum; i++) {
			vertaxs[i].id = i;
			vertaxs[i].parent = -1;
			vertaxs[i].closed = 0;
			//INFDIS表示无穷大，此路不通
			vertaxs[i].cost = INFDIS;
		}
		Lane* startLane = getLane(start.roadId, start.laneSectionId, start.laneId);
		Lane* endLane = getLane(end.roadId, end.laneSectionId, end.laneId);
		if (startLane == nullptr || endLane == nullptr) {
			std::cout << "起始路径不存在" << std::endl;
			return false;
		}
		//特殊情况：当起点和终点在同一条道路节点时，并且终点在前起点在后，此时只能转一圈到达终点,需要两次将道路节点从open表中移除
		//每从open表中移除一次目的节点，remainRemovalTimes减少一，如果remainRemovalTimes并且到达目标节点，说明找到路径。默认为一，特殊情况下为二
		int remainRemovalTimes = 1;
		if (startLane == endLane && start.offset > end.offset) {
			remainRemovalTimes = 2;
			//std::cout << "相同车道寻路。。"<<std::endl;
		}
		int startId = startLane->getUniqueId();
		int endId = endLane->getUniqueId();
		vertaxs[startId].cost = 0;
		vertaxs[startId].startOffset = start.offset;
		open.push(&vertaxs[startId]);
		while (!open.empty()) {
			//从open表中弹出最小路径代价的节点
			struct Vertax *minCostNode = open.top();
			Lane* minLane = getLane(nodes[minCostNode->id].roadId, nodes[minCostNode->id].laneSectionId, nodes[minCostNode->id].laneId);
			//std::cout << "minCostNodeID:" << minCostNode->id << std::endl;
			open.pop();
			//如果最小代价的节点已经无穷大，则不必要搜索
			if (vertaxs[minCostNode->id].cost >= INFDIS) {
				break;
			}
			//将此节点closed置1表示从start节点到该节点的最短路径已经找到
			vertaxs[minCostNode->id].closed = 1;
			//如果到达目的节点,则需要判断
			if (minCostNode->id == endId) {
				remainRemovalTimes--;
				if (remainRemovalTimes == 0) {
					break;
				}
				else {
					vertaxs[minCostNode->id].closed = 0;
				}
			}
			//cost表示从起点到当前道路的startOffset的代价
			float weight;
			weight = getLaneLength(minCostNode->id) - vertaxs[minCostNode->id].startOffset;
			//minCostNode节点的所有后继节点
			Edge *neiborNode = nodes[minCostNode->id].next;
			while (neiborNode) {
				//从open表中弹出最小路径代价的节点
				int neiborNodeId = neiborNode->id;
				Lane* neiborLane = getLane(nodes[neiborNodeId].roadId, nodes[neiborNodeId].laneSectionId, nodes[neiborNodeId].laneId);
				//如果该节点是driving && 该节点最短路径未找到 && 该节点的前驱节点不是minCostNode
				if (neiborLane->getType() == "driving" && vertaxs[neiborNodeId].closed == 0 && vertaxs[neiborNodeId].parent != minCostNode->id) {
					//如果是特殊情况或代价更短
					if (vertaxs[neiborNodeId].cost == 0 || vertaxs[neiborNodeId].cost > vertaxs[minCostNode->id].cost + weight) {
						//换道标志
						vertaxs[neiborNodeId].flag = 0;
						//如果没换道则是从起点开始
						vertaxs[neiborNodeId].startOffset = 0;
						//更新代价
						vertaxs[neiborNodeId].cost = vertaxs[minCostNode->id].cost + weight;
						//如果该节点没有在open表中,放入open表
						if (vertaxs[neiborNodeId].parent == -1) {
							open.push(&vertaxs[neiborNodeId]);
						}
						//更新父节点
						vertaxs[neiborNodeId].parent = minCostNode->id;
					}
				}
				neiborNode = neiborNode->next;
			}
			//考虑换道情况
			int laneId = minLane->getId();
			//s[0]表示左侧车道的车道编号，s[1]表示右侧车道的车道编号
			int s[2];
			//如果位于右侧车道
			if (laneId < 0) {
				s[0] = getLaneUniqueId(nodes[minCostNode->id].roadId, nodes[minCostNode->id].laneSectionId, nodes[minCostNode->id].laneId + 1);
				s[1] = getLaneUniqueId(nodes[minCostNode->id].roadId, nodes[minCostNode->id].laneSectionId, nodes[minCostNode->id].laneId - 1);
			}
			//如果位于左侧车道
			if (laneId > 0) {
				s[0] = getLaneUniqueId(nodes[minCostNode->id].roadId, nodes[minCostNode->id].laneSectionId, nodes[minCostNode->id].laneId - 1);
				s[1] = getLaneUniqueId(nodes[minCostNode->id].roadId, nodes[minCostNode->id].laneSectionId, nodes[minCostNode->id].laneId + 1);
			}
			//定义越道代价
			int crossLaneCost = 2.5;
			for (int i = 0; i < 2; i++) {
				//如果该节点编号有效 && //如果是可行使道路 && 该节点最短路径未找到 && 该节点的前驱节点不是minCostNode
				if (s[i] != -1 && getLaneType(s[i]) == "driving" && vertaxs[s[i]].closed == 0 && vertaxs[s[i]].parent != minCostNode->id) {
					//如果代价更短
					if (vertaxs[s[i]].cost > vertaxs[minCostNode->id].cost + crossLaneCost) {
						//更新代价
						vertaxs[s[i]].cost = vertaxs[minCostNode->id].cost + crossLaneCost;
						//换道标志
						vertaxs[s[i]].flag = 1;
						//如果该节点没有在open表中,放入open表
						if (vertaxs[s[i]].parent == -1) {
							open.push(&vertaxs[s[i]]);
						}
						//更新父节点
						vertaxs[s[i]].parent = minCostNode->id;
						//如果换道，则认为是从前一车道的startOffset开始，因为换道是在同一laneSection中进行，所以不会越界
						vertaxs[s[i]].startOffset = vertaxs[minCostNode->id].startOffset;
					}
				}
			}
		}

		//如果未找到最短路径，则退出
		if (vertaxs[endId].closed == 0) {
			std::cout << "can't reach!" << std::endl;
			return false;
		}
		//保存搜索的路径编号，从终点到起点
		std::vector<int> uniqueLaneIdFromEndToStart;
		uniqueLaneIdFromEndToStart.push_back(endId);
		std::cout << "(" << end.roadId << "," << end.laneSectionId << "," << end.laneId << ")";
		int i = vertaxs[endId].parent;
		while (i != -1) {
			std::cout << "<--(" << nodes[vertaxs[i].id].roadId << "," << nodes[vertaxs[i].id].laneSectionId << "," << nodes[vertaxs[i].id].laneId << ")";
			uniqueLaneIdFromEndToStart.push_back(vertaxs[i].id);
			if (vertaxs[i].id == endId) {
				break;
			}
			i = vertaxs[i].parent;
		}
		std::cout << std::endl;
		if (debug) {
			//输出路径点
			int pathSize = uniqueLaneIdFromEndToStart.size();
			std::fstream path(prefix_output_path+"path.txt", std::ios::out);
			std::fstream pathForPlot(prefix_output_path+"pathForPlot.txt", std::ios::out);
			int total = 0;
			for (int i = pathSize - 1; i >= 0; i--) {
				int uniqueId = uniqueLaneIdFromEndToStart[i];
				Lane* lane = getLane(nodes[uniqueId].roadId, nodes[uniqueId].laneSectionId, nodes[uniqueId].laneId);
				float laneLength = getLaneLength(nodes[uniqueId].roadId, nodes[uniqueId].laneSectionId, nodes[uniqueId].laneId);
				std::vector<Point> centerLine = lane->getCenterLine();
				int startIndex;
				int endIndex = centerLine.size();
				int centerLineSize = endIndex;
				//起点道路
				if (i == pathSize - 1) {
					startIndex = ceil(start.offset / laneLength * centerLineSize);
				}
				else {
					startIndex = ceil(vertaxs[uniqueId].startOffset / laneLength * centerLineSize);
				}
				//终点道路
				if (i == 0) {
					endIndex = end.offset / laneLength * centerLineSize;
				}//如果下一条道是由前一条道换道过来的，换道只在同一个laneSection中换，因此道路的长度相同，点的个数也相同
				else if (vertaxs[uniqueLaneIdFromEndToStart[i - 1]].flag == 1) {
					endIndex = vertaxs[uniqueLaneIdFromEndToStart[i - 1]].startOffset / laneLength * centerLineSize;
				}
				std::string s = "10.0,1,0,1,0,0,";
				int laneId = lane->getId();
				if (laneId == -1) {
					laneId = 2;
				}
				else {
					laneId = 4;
				}
				for (int j = startIndex; j < endIndex; j++) {
					pathForPlot << centerLine[j].getX() << "," << centerLine[j].getY() << std::endl;
					path << total++ << "," << centerLine[j].getX() << "," << centerLine[j].getY() << "," << s;
					path << laneId << ",0,0;" << std::endl;
				}
			}
			path.close();
		}

		return true;
	}*/
	Position OpenDrive::findPosition(float x, float y)
	{
		float mindis = -1;
		std::string roadId;
		int laneSectionId;
		int laneId;
		float offset;
		float x1, y1;
		for (auto it : roads) {
			Road &road = it.second;
			int laneSectionLength = road.getLanes().getLaneSectionsSize();
			Lanes& lanes = road.getLanes();
			for (int i = 0; i < laneSectionLength; i++) {
				LaneSection& laneSection = lanes.getLaneSectionById(i);
				LeftCenterRight& left = laneSection.getLeft();
				LeftCenterRight& right = laneSection.getRight();
				int laneSize = left.getLaneSize();
				for (int j = 1; j <= laneSize; j++) {
					Lane& lane = left.getLane(j);
					if (lane.getType() == "driving") {
						const std::vector<Point>& centerLine = lane.getCenterLine();
						int centerLineSize = centerLine.size();
						for (int k = 0; k < centerLineSize; k++) {
							float xx = x - centerLine[k].getX();
							float yy = y - centerLine[k].getY();
							float dis = xx * xx + yy * yy;
							if (mindis == -1 || dis < mindis) {
								x1 = centerLine[k].getX();
								y1 = centerLine[k].getY();
								roadId = it.first;
								laneSectionId = i;
								laneId = j;
								offset = centerLine[0].getS() - centerLine[k].getS();
								mindis = dis;
							}
						}
					}
				}
				laneSize = right.getLaneSize();
				for (int j = 1; j <= laneSize; j++) {
					Lane& lane = right.getLane(-j);
					if (lane.getType() == "driving") {
						const std::vector<Point> &centerLine = lane.getCenterLine();
						int centerLineSize = centerLine.size();
						for (int k = 0; k < centerLineSize; k++) {
							float xx = x - centerLine[k].getX();
							float yy = y - centerLine[k].getY();
							float dis = xx * xx + yy * yy;
							if (mindis == -1 || dis < mindis) {
								x1 = centerLine[k].getX();
								y1 = centerLine[k].getY();
								roadId = it.first;
								laneSectionId = i;
								laneId = -j;
								offset = centerLine[k].getS() - centerLine[0].getS();
								mindis = dis;
							}
						}
					}
				}
			}
		}
		std::cout << x1 << "," << y1 << std::endl;
		Position position(roadId, laneSectionId, laneId, offset);
		return position;
	}
	bool OpenDrive::findPath(Position start, Position end, bool append, bool turnRound)
	{
		if (valid() == false) {
			std::cout << "OpenDrive未初始化" << std::endl;
			return false;
		}
		return AstarPlan(start, end, append, turnRound);
	}
	bool OpenDrive::findPath(float x1, float y1, float x2, float y2, bool append, bool turnRound)
	{
		if (valid() == false) {
			std::cout << "OpenDrive未初始化" << std::endl;
			return false;
		}

		std::fstream out;
		if (append) {
			out.open(prefix_output_path+"position.txt", std::ios::app);
		}
		else {
			out.open(prefix_output_path+"position.txt", std::ios::out);
		}
		out << x1 << "," << y1 << "," << Z_ << std::endl;
		out << x2 << "," << y2 << "," << Z_ << std::endl;
		out.close();
		Position start = findPosition(x1, y1);
		Position end = findPosition(x2, y2);
		std::cout << start.roadId << "," << start.laneSectionId << "," << start.laneId << std::endl;
		std::cout << end.roadId << "," << end.laneSectionId << "," << end.laneId << std::endl;
		return findPath(start, end, append, turnRound);
	}
	bool OpenDrive::findPath(keyPoint startp, keyPoint endp, bool append, bool turnRound)
	{
		return findPath(startp.x, startp.y, endp.x, endp.y, append, turnRound);
	}
	void OpenDrive::outputPointsToFile() {

		std::fstream refPointsOut(prefix_output_path+"geometry.txt", std::ios::out);
		std::fstream laneLineOut(prefix_output_path+"driving_lane.txt", std::ios::out);
		std::fstream centerLineOut(prefix_output_path+"driving_center_line.txt", std::ios::out);
		for (auto &it : roads) {
			Road &road = it.second;
			//输出参考线的点
			std::vector<std::shared_ptr<Geometry>> &geos = road.getPlanView().getGeometries();
			for (auto iter = geos.begin(); iter != geos.end(); iter++) {
				const std::vector<Point>& ps = (*iter)->getPoints();
				int size = ps.size();
				for (int i = 0; i < size; i++) {
					refPointsOut << ps[i].getX() << "," << ps[i].getY() << "," << Z_ << std::endl;
				}
			}
			std::vector<LaneSection> &laneSections = road.getLanes().getLaneSections();
			for (int i = 0, size = laneSections.size(); i < size; i++) {
				LaneSection &laneSection = laneSections[i];
				//std::vector<Lane>laneLeft = laneSection.getLeft()->getLanes();

				for (auto &item : laneSection.getLeft().getLanes()) {
					Lane &lane = item.second;
					if (lane.getType() == "driving") {
						//if (lane.getType() != "1driving") {
						const std::vector<Point> &laneLine = lane.getRightLine();
						const std::vector<Point> &centerLine = lane.getCenterLine();
						int laneLineLength = laneLine.size();
						int centerLineLength = centerLine.size();
						for (int j = 0; j < laneLineLength; j++)
							laneLineOut << laneLine[j].getX() << "," << laneLine[j].getY() << "," << Z_ << std::endl;
						for (int j = 0; j < centerLineLength; j++)
							centerLineOut << centerLine[j].getX() << "," << centerLine[j].getY() << "," << Z_ << std::endl;
					}
				}
				for (auto &item : laneSection.getRight().getLanes()) {
					Lane &lane = item.second;
					if (lane.getType() == "driving") {
						//if (lane.getType() != "1driving") {
						const std::vector<Point> &laneLine = lane.getRightLine();
						const std::vector<Point> &centerLine = lane.getCenterLine();
						int laneLineLength = laneLine.size();
						int centerLineLength = centerLine.size();
						for (int j = 0; j < laneLineLength; j++)
							laneLineOut << laneLine[j].getX() << "," << laneLine[j].getY() << "," << Z_ << std::endl;
						for (int j = 0; j < centerLineLength; j++)
							centerLineOut << centerLine[j].getX() << "," << centerLine[j].getY() << "," << Z_ << std::endl;
					}
				}
			}
		}
		refPointsOut.close();
		laneLineOut.close();
		centerLineOut.close();
	}
	void OpenDrive::outputRefPointsToFileByNum()
	{
		std::string roadId;
		for (auto &it : roads) {
			Road &road = it.second;
			std::string roadId = it.first;
			std::fstream roadref(prefix_output_path+R"(roadRef/)" + roadId + ".txt", std::ios::out);
			std::vector<std::shared_ptr<Geometry>> geos = road.getPlanView().getGeometries();
			for (auto iter = geos.begin(); iter != geos.end(); iter++) {
				const std::vector<Point>& ps = (*iter)->getPoints();
				int size = ps.size();
				for (int i = 0; i < size; i++) {
					roadref << ps[i].getX() << "," << ps[i].getY() << std::endl;
				}
			}
			roadref.close();
		}
	}
	void OpenDrive::format(int uniqueId) {
		//因为有可能length没有计算，所以先计算然后format
		getLaneLength(uniqueId);
		getLane(uniqueId).format();
	}
	void OpenDrive::print()
	{
		std::cout << "打印:" << roads.size() << std::endl;
		for (int i = 0, size = nodes.size(); i < size; i++) {
			Lane& lane = getLane(nodes[i].roadId, nodes[i].laneSectionId, nodes[i].laneId);
			if (lane.getType() == "driving") {
				std::cout << lane.getUniqueId() << "(" << nodes[i].roadId << "," << nodes[i].laneSectionId << "," << nodes[i].laneId << "," << lane.getType() + ")";
				Edge * edge = nodes[lane.getUniqueId()].next;
				while (edge) {
					std::cout << "-->" << edge->id << "(" << nodes[edge->id].roadId << "," << nodes[edge->id].laneSectionId << "," << nodes[edge->id].laneId << "," + getLaneType(edge->id) + ")";
					edge = edge->next;
				}
				std::cout << std::endl;
			}
		}
	}

	Road& OpenDrive::getRoad(std::string roadId)
	{
			return roads[roadId];
	}


	void OpenDrive::calOneSideLine(LeftCenterRight &lr, bool left, const std::vector<Point>& refPoints, float laneSectionStartS, float laneSectionEndS)
	{
		int laneNum = lr.getLaneSize();
		if (laneNum == 0)
			return;
		int sign = -1;
		if (left)
			sign = -sign;
		bool reverse = true, noReverse = false, start = true, end = false;
		int startIndexOfLaneSection = findIndex(refPoints, laneSectionStartS, noReverse, start);
		int endIndexOfLaneSection = findIndex(refPoints, laneSectionEndS, noReverse, end);
		//laneSectionPoints是当前车道计算时的参考车道，即上一条车道，初始为参考线
		std::vector<Point> laneSectionPoints(refPoints.begin() + startIndexOfLaneSection, refPoints.begin() + endIndexOfLaneSection + 1);
		/*laneSectionPoints改这个，加上width和type信息*/
		std::vector<Point> centerLine = laneSectionPoints;
		for (int i = 1; i <= laneNum; i++) {
			Lane &lane = lr.getLane(sign*i);
			const std::vector<Width>& widths = lane.getWidths();
			int widthNum = widths.size();

			float offsetRatio = 0.5f;
			if (i == 1) {
				if (left) {
					std::reverse(laneSectionPoints.begin(), laneSectionPoints.end());
					std::reverse(centerLine.begin(), centerLine.end());
				}
			}
			//设置左车道线
			lane.setLeftLine(laneSectionPoints);

			//special特殊处理标志，如果当前车道与参考车道在某一处存在距离小于一个阈值，则为true，表示当前车道的中心线需要特殊处理
			bool special = false;
			int startIndex, endIndex;
			float normalWidth = 0.0;
			std::vector<float> offsetWidths;
			for (int j = 0; j < widthNum; j++) {
				Width width = widths[j];
				float widthStartS = width.getSOffset() + laneSectionStartS;
				float widthEndS;
				if (j == widthNum - 1)
					widthEndS = laneSectionEndS;
				else
					widthEndS = widths[j + 1].getSOffset() + laneSectionStartS;
				if (left) {
					if (j == 0) {
						startIndex = findIndex(laneSectionPoints, widthEndS, reverse, start);
						endIndex = findIndex(laneSectionPoints, widthStartS, reverse, end);
					}
					else {
						endIndex = startIndex - 1;
						startIndex = findIndex(laneSectionPoints, widthEndS, reverse, start);
					}
				}
				else {
					if (j == 0) {
						startIndex = findIndex(laneSectionPoints, widthStartS, noReverse, start);
						endIndex = findIndex(laneSectionPoints, widthEndS, noReverse, end);
					}
					else {
						startIndex = endIndex + 1;
						endIndex = findIndex(laneSectionPoints, widthEndS, noReverse, end);
					}
				}
				normalWidth = lane.getNormalWidth();
				float a = width.getA();
				float b = width.getB();
				float c = width.getC();
				float d = width.getD();

				//正常情况下中心线是上条车道偏移一半距离得到的
				calOffset(laneSectionPoints, startIndex, endIndex, centerLine, startIndex, offsetRatio*a, offsetRatio*b, offsetRatio*c, offsetRatio*d, widthStartS, noReverse, left);
				//设定阈值，如果距离小于阈值，则进行特殊处理
				special = special | calOffset(laneSectionPoints, startIndex, endIndex, laneSectionPoints, startIndex, a, b, c, d, widthStartS, noReverse, left, 0.5);
			}
			//设置右车道线
			lane.setRightLine(laneSectionPoints);
			////如果该车道线与其他车道线存在重合，则需要特殊计算中心线
			//if (special) {
			//	float MIN_LANE_WIDTH = 0.1;
			//	Lane* preLane = lr->getLane(sign*(i - 1));
			//	Lane* nextLane = lr->getLane(sign*(i + 1));
			//	if (i == 1 || preLane->getType() != "driving") {
			//		//右车道线向左车道线方向偏移一段距离
			//		if (normalWidth < MIN_LANE_WIDTH) {
			//			if (nextLane != nullptr)
			//				normalWidth = nextLane->getNormalWidth();
			//			if (normalWidth < MIN_LANE_WIDTH && preLane != nullptr)
			//				normalWidth = preLane->getNormalWidth();
			//			if (normalWidth < MIN_LANE_WIDTH)
			//				normalWidth = 3.25;
			//		}
			//		calOffset(lane->getLeftLine(), 0, laneSectionPoints.size() - 1, centerLine, 0, offsetRatio*normalWidth, 0, 0, 0, laneSectionStartS, noReverse, left);
			//	}
			//	else if (i == laneNum || nextLane->getType() != "driving") {
			//		//左车道线向右车道线方向偏移一段距离
			//		if (normalWidth < MIN_LANE_WIDTH) {
			//			if (preLane != nullptr)
			//				normalWidth = preLane->getNormalWidth();
			//			if (normalWidth < MIN_LANE_WIDTH && nextLane != nullptr)
			//				normalWidth = nextLane->getNormalWidth();
			//			if (normalWidth < MIN_LANE_WIDTH)
			//				normalWidth = 3.25;
			//		}
			//		calOffset(lane->getRightLine(), 0, laneSectionPoints.size() - 1, centerLine, 0, offsetRatio*normalWidth, 0, 0, 0, laneSectionStartS, noReverse, !left);
			//	}
			//	else {
			//		std::cout << "还有其他情况？" << std::endl;
			//	}
			//}
			////设置中心线
			lane.setCenterLine(centerLine);
			format(lane.getUniqueId());
		}
	}

	//对refPoints[startIndex,endIndex]范围内的点计算偏移
	//offset表示起始偏移，reverse表示是否和当前refPoints的方向相反，left表示是否向左偏移,左右偏移是相对参考线方向
	//如果计算的距离存在小于阈值，则范围true表示中心线特殊处理
	bool OpenDrive::calOffset(const std::vector<Point>& refPoints, int startIndex, int endIndex, std::vector<Point>& res, int startIndex2, float a, float b, float c, float d, float offset, bool reverse, bool left, float threshold)
	{
		float s;
		bool flag = false;
		int size = endIndex - startIndex + 1;
		int start = startIndex, step = 1;
		float theta = -mtc::M_PI / 2.0;
		if (reverse) {
			start = endIndex; step = -1;
		}
		if (left)	theta = -theta;
		// std::vector<Point>res(size);
		for (int i = 0; i < size; i++) {
			Point p = refPoints[start];
			s = p.getS() - offset;
			float dis = a + b * s + c * pow(s, 2) + d * pow(s, 3);
			//距离小于给定的阈值，则中心线特殊处理,如果阈值小于等于0，则不判断（重载函数使用）
			if (threshold > 0 && fabs(dis) < threshold) {
				flag = true;
			}
			res[startIndex2].setS(p.getS());
			res[startIndex2].setTheta(p.getTheta());
			res[startIndex2].setX(p.getX() + dis * cos(p.getTheta() + theta));
			res[startIndex2].setY(p.getY() + dis * sin(p.getTheta() + theta));
			startIndex2++;
			start += step;
		}
		return flag;

	}
	//此函数只计算偏移，没有距离小于阈值返回true的功能
	void OpenDrive::calOffset(const std::vector<Point>& refPoints, int startIndex, int endIndex, std::vector<Point>& res, int startIndex2, float a, float b, float c, float d, float offset, bool reverse, bool left)
	{
		calOffset(refPoints, startIndex, endIndex, res, startIndex2, a, b, c, d, offset, reverse, left, -1);
	}
	//reverse==true，refPoints中的point按照s递减存放；否则递增存放
	//start==true，返回s右边的第一个点
	//start==false，返回s左边的第一个点
	int OpenDrive::findIndex(const std::vector<Point>& refPoints, float s, bool reverse, bool start)
	{
		int startIndex = 0;
		int endIndex = refPoints.size() - 1;
		int midIndex;
		while (startIndex <= endIndex) {
			midIndex = (startIndex + endIndex) / 2;
			if (fabs(refPoints[midIndex].getS() - s) <= mtc::FloatMinErrorValue) {
				return midIndex;
			}
			if (refPoints[midIndex].getS() - s > mtc::FloatMinErrorValue && reverse || s - refPoints[midIndex].getS() > mtc::FloatMinErrorValue && !reverse) {
				startIndex = midIndex + 1;
			}
			else {
				endIndex = midIndex - 1;
			}
		}
		if (start) {
			if (startIndex == refPoints.size())
				startIndex = refPoints.size() - 1;
			else if (startIndex == -1)	startIndex = 0;
			return startIndex;
		}
		if (endIndex == refPoints.size())
			endIndex = refPoints.size() - 1;
		else if (endIndex == -1)
			endIndex = 0;
		return endIndex;
		/*int index = int(s / stepLength);
		int size = refPoints.size();
		if (index >= size) {
			if (start)
				return -1;
			return size - 1;
		}
		float refS = refPoints[index].getS();
		if (refS == s)
			return index;
		if (refS < s) {
			while (1) {
				index++;
				if (index == size) {
					if (start) return -1;
					return index - 1;
				}
				refS = refPoints[index].getS();
				if (refS == s)
					return index;
				if (refS > s) {
					if (start) return index;
					return index - 1;
				}
			}
		}
		else {
			std::cout << "应该没有refS>s的情况" << std::endl;
		}
		return 0;*/
	}

	//void OpenDrive::calOffsetWidths(std::vector<Point>& points, int startIndex, int endIndex, float offset, float a, float b, float c, float d, std::vector<float>& offsetWidths,bool assign)
	//{
	//	int size = endIndex - startIndex + 1;;
	//	offsetWidths.reserve(size);
	//	for (int i = startIndex; i <= endIndex; i++) {
	//		float s = points[i].getS() - offset;
	//		float dis = a + b * s + c * pow(s, 2) + d * pow(s, 3);
	//		offsetWidths.push_back(dis);
	//		if (assign) {
	//			points[i].setWidth(dis);
	//		}
	//	}
	//}

	void OpenDrive::calLaneLineAndCenterLine()
	{
		std::unordered_map<std::string, Road>::iterator it;
		std::fstream out(prefix_output_path+"geometry1.txt", std::ios::out);
		for (it = roads.begin(); it != roads.end(); it++) {
			Road &road = it->second;
			Lanes &lanes = road.getLanes();
			//geoPoints 取出参考线中所有的点
			std::vector<Point> refPoints;
			std::vector<std::shared_ptr<Geometry>> geos = road.getPlanView().getGeometries();
			for (auto iter = geos.begin(); iter != geos.end(); iter++) {
				const std::vector<Point>& ps = (*iter)->getPoints();
				refPoints.insert(refPoints.end(), ps.begin(), ps.end());
			}

			std::vector<LaneOffset> laneOffsets = lanes.getLaneOffsets();
			//偏移参考线的点
			int startIndex, endIndex;
			if (laneOffsets.size() != 0) {
				int laneOffsetsSize = laneOffsets.size();
				for (int i = 0; i < laneOffsetsSize; i++) {
					float startS = laneOffsets[i].getS();
					float endS;
					if (i == laneOffsetsSize - 1) endS = road.getLength();
					else endS = laneOffsets[i + 1].getS();
					bool start = true, end = false, noRreverse = false, toLeft = true;
					if (i == 0) {
						startIndex = findIndex(refPoints, startS, noRreverse, start);
						endIndex = findIndex(refPoints, endS, noRreverse, end);
					}
					else {
						startIndex = endIndex + 1;
						endIndex = findIndex(refPoints, endS, noRreverse, end);
					}

					float a = laneOffsets[i].getA();
					float b = laneOffsets[i].getB();
					float c = laneOffsets[i].getC();
					float d = laneOffsets[i].getD();
					calOffset(refPoints, startIndex, endIndex, refPoints, startIndex, a, b, c, d, startS, noRreverse, toLeft);

				}
			}
			//将偏离后的参考线输出
			int refPointsSize = refPoints.size();
			for (int ii = 0; ii < refPointsSize; ii++) {
				out << refPoints[ii].getX() << "," << refPoints[ii].getY() << "," << Z_ << std::endl;
			}
			//计算车道线
			int laneSectionsSize = lanes.getLaneSectionsSize();
			for (int i = 0; i < laneSectionsSize; i++) {

				LaneSection & laneSection = lanes.getLaneSectionById(i);
				float laneSectionS = laneSection.getS();
				LeftCenterRight& left = laneSection.getLeft();
				LeftCenterRight& right = laneSection.getRight();
				float laneSectionEndS;
				if (i == laneSectionsSize - 1) {
					laneSectionEndS = road.getLength();
				}
				else {
					laneSectionEndS = lanes.getLaneSectionById(i + 1).getS();
				}
				calOneSideLine(left, true, refPoints, laneSectionS, laneSectionEndS);
				calOneSideLine(right, false, refPoints, laneSectionS, laneSectionEndS);
			}
		}
		out.close();
	}

	bool OpenDrive::buildLaneLinkBetweenTwoLane(std::string startRoadId, int startLaneSectionId, int startLaneId, std::string endRoadId, int endLaneSectionId, int endLaneId)
	{
		
		if (false == laneExist(startRoadId,startLaneSectionId,startLaneId)) {
			std::cerr << "未找到构建连接的startLane:(" + startRoadId + "," << startLaneSectionId << "," << startLaneId << ")" << std::endl;
			return false;
		}
		if (false == laneExist(endRoadId, endLaneSectionId, endLaneId)) {
			std::cerr << "未找到构建连接的endLane:(" + endRoadId + "," << endLaneSectionId + "," << endLaneId << ")" << std::endl;
			return false;
		}
		Lane &startLane = getLane(startRoadId, startLaneSectionId, startLaneId);
		Lane &endLane = getLane(endRoadId, endLaneSectionId, endLaneId);
		int startId = startLane.getUniqueId();
		int endId = endLane.getUniqueId();
		Edge* p = nodes[startId].next;
		if (p == nullptr) {
			nodes[startId].next = new Edge(endId);
			//startLane->next = new Edge(endRoadId,endLaneSectionId,endLaneId);
			return true;
		}
		while (1) {
			if (p->id == endId)
				return true;
			if (p->next == nullptr)
				break;
			p = p->next;
		}
		p->next = new Edge(endId);
		return true;
	}
	bool OpenDrive::buildOneSideLink(std::string roadId, int laneSectionId, bool left)
	{
		//左侧车道的行驶方向与参考线相反，右侧车道的行驶方向与参考线相同
		//openDrive中车道的前驱和后继是相对于road的参考线方向定义的，而不是车道的行驶方向
		Road& road = getRoad(roadId);
		const Predecessor* const roadPre = road.getRoadLink().getPredecessor(); //获取前驱
		const Successor* const roadSuccessor = road.getRoadLink().getSuccessor(); //获取后继
		std::string roadLinkPreId;
		std::string roadLinkPreType;
		std::string roadLinkPreContactPoint;
		std::string roadLinkSuccId;
		std::string roadLinkSuccType;
		std::string roadLinkSuccContactPoint;

		if (roadPre != nullptr) {
			roadLinkPreId = roadPre->getId();		//前驱raod id or junction id
			roadLinkPreType = roadPre->getType();	//前驱type :raod or junction
			roadLinkPreContactPoint = roadPre->getContactPoint();
		}

		if (roadSuccessor != nullptr) {
			roadLinkSuccId = roadSuccessor->getId();		//后继 raod id or junction id
			roadLinkSuccType = roadSuccessor->getType();	//后继 type :raod or junction
			roadLinkSuccContactPoint = roadSuccessor->getContactPoint();
		}

		std::unordered_map<int, Lane> *lanes;
		if (left) {
			lanes = &(road.getLanes().getLaneSectionById(laneSectionId).getLeft().getLanes());
		}
		else {
			lanes = &(road.getLanes().getLaneSectionById(laneSectionId).getRight().getLanes());
		}
		for (auto it = lanes->begin(); it != lanes->end(); it++) {
			Lane &lane = it->second;
			const Link &link = lane.getLink();
			const Predecessor* const lanePre = link.getPredecessor();
			const Successor* const laneSucc = link.getSuccessor();
			//当前车道编号
			int laneId = lane.getId();
			//后继车道编号
			int laneSuccId;
			//后继车道所在的laneSection编号
			int laneSectionIdOfLaneSucc = -1;
			//后继车道所在的raod编号
			std::string RoadIdOfLaneSucc = "";
			//前驱车道编号
			int lanePreId;
			//前驱车道所在的laneSection编号
			int laneSectionIdOfLanePre = -1;
			//前驱车道所在的raod编号
			std::string RoadIdOfLanePre = "";

			//将每个车道作为节点创建邻接表，每个车道节点连接的边表示该车道按车道方向行驶直接相连的下一条车道
			if (lanePre != nullptr) {
				if (0 == laneSectionId) {
					if (roadPre == nullptr) {
						std::cerr << "road:" << roadId << "没有找到前驱" << std::endl;
						return false;
					}
					lanePreId = atoi(lanePre->getId().c_str());
					if (roadLinkPreType == "road" && false == buildLaneLinkBetweenTwoRoad(roadId, roadLinkPreId, roadLinkPreContactPoint, laneId, lanePreId)) {
						return false;
					}
				}
				else {
					if (left) {
						laneSuccId = atoi(lanePre->getId().c_str());
						laneSectionIdOfLaneSucc = laneSectionId - 1;
						RoadIdOfLaneSucc = roadId;
						//构建连接关系
						if (false == buildLaneLinkBetweenTwoLane(roadId, laneSectionId, laneId, RoadIdOfLaneSucc, laneSectionIdOfLaneSucc, laneSuccId)) {
							return false;
						}
					}
					else {
						lanePreId = atoi(lanePre->getId().c_str());
						laneSectionIdOfLanePre = laneSectionId - 1;
						RoadIdOfLanePre = roadId;
						//构建连接关系
						if (false == buildLaneLinkBetweenTwoLane(RoadIdOfLanePre, laneSectionIdOfLanePre, lanePreId, roadId, laneSectionId, laneId))
							return false;
					}
				}
			}
			if (laneSucc != nullptr) {
				if (laneSectionId == getRoad(roadId).getLanes().getLaneSections().size() - 1) {
					if (roadSuccessor == nullptr) {
						std::cerr << "road:" << roadId << "没有找到后继" << std::endl;
						return false;
					}
					laneSuccId = atoi(laneSucc->getId().c_str());
					if (roadLinkSuccType == "road" && false == buildLaneLinkBetweenTwoRoad(roadId, roadLinkSuccId, roadLinkSuccContactPoint, laneId, laneSuccId)) {
						return false;
					}
				}
				else {
					if (left) {
						lanePreId = atoi(laneSucc->getId().c_str());
						laneSectionIdOfLanePre = laneSectionId + 1;
						RoadIdOfLanePre = roadId;
						//构建连接关系
						if (false == buildLaneLinkBetweenTwoLane(RoadIdOfLanePre, laneSectionIdOfLanePre, lanePreId, roadId, laneSectionId, laneId))
							return false;
					}
					else {
						laneSuccId = atoi(laneSucc->getId().c_str());
						laneSectionIdOfLaneSucc = laneSectionId + 1;
						RoadIdOfLaneSucc = roadId;
						//构建连接关系
						if (false == buildLaneLinkBetweenTwoLane(roadId, laneSectionId, laneId, RoadIdOfLaneSucc, laneSectionIdOfLaneSucc, laneSuccId)) {
							return false;
						}
					}
				}

			}

		}
		return true;
	}
	//构建两个road之间的的lane的连接关系
	bool OpenDrive::buildLaneLinkBetweenTwoRoad(std::string incomingRoadId, std::string connectingRoadId, std::string contactPoint, int from, int to)
	{
		//按照车道lane的行驶方向定义出开始车道和结束车道所在的road编号和laneSection编号和车道lane编号
		std::string startRoadId;
		std::string endRoadId;
		int startLaneSectionId;
		int endLaneSectionId;
		int startLaneId;
		int endLaneId;

		if ("start" == contactPoint) {
			if (from > 0 && to > 0) {
				startRoadId = connectingRoadId;
				startLaneSectionId = 0;
				startLaneId = to;

				endRoadId = incomingRoadId;
				endLaneSectionId = getRoad(endRoadId).getLanes().getLaneSections().size() - 1;
				endLaneId = from;
			}
			else if (from < 0 && to < 0) {
				startRoadId = incomingRoadId;
				startLaneSectionId = getRoad(startRoadId).getLanes().getLaneSections().size() - 1;
				startLaneId = from;

				endRoadId = connectingRoadId;
				endLaneSectionId = 0;
				endLaneId = to;
			}
			else if (from > 0 && to < 0) {
				startRoadId = incomingRoadId;
				startLaneSectionId = 0;
				startLaneId = from;

				endRoadId = connectingRoadId;
				endLaneSectionId = 0;
				endLaneId = to;
			}
			else if (from < 0 && to > 0) {
				startRoadId = connectingRoadId;
				startLaneSectionId = 0;
				startLaneId = to;

				endRoadId = incomingRoadId;
				endLaneSectionId = 0;
				endLaneId = from;
			}

		}
		else {
			if (from > 0 && to > 0) {
				startRoadId = incomingRoadId;
				startLaneSectionId = 0;
				startLaneId = from;

				endRoadId = connectingRoadId;
				endLaneSectionId = getRoad(endRoadId).getLanes().getLaneSections().size() - 1;
				endLaneId = to;
			}
			else if (from < 0 && to < 0) {
				startRoadId = connectingRoadId;
				startLaneSectionId = getRoad(startRoadId).getLanes().getLaneSections().size() - 1;
				startLaneId = to;

				endRoadId = incomingRoadId;
				endLaneSectionId = 0;
				endLaneId = from;
			}
			else if (from > 0 && to < 0) {
				startRoadId = connectingRoadId;
				startLaneSectionId = getRoad(startRoadId).getLanes().getLaneSections().size() - 1;
				startLaneId = to;

				endRoadId = incomingRoadId;
				endLaneSectionId = getRoad(endRoadId).getLanes().getLaneSections().size() - 1;
				endLaneId = from;
			}
			else if (from < 0 && to > 0) {
				startRoadId = incomingRoadId;
				startLaneSectionId = getRoad(startRoadId).getLanes().getLaneSections().size() - 1;
				startLaneId = from;

				endRoadId = connectingRoadId;
				endLaneSectionId = getRoad(endRoadId).getLanes().getLaneSections().size() - 1;
				endLaneId = to;
			}
		}
		//构建连接关系
		return buildLaneLinkBetweenTwoLane(startRoadId, startLaneSectionId, startLaneId, endRoadId, endLaneSectionId, endLaneId);
	}
	//构建车道间的连接关系
	bool OpenDrive::buildMap()
	{
		if (!valid()) {
			return false;
		}
		//遍历road
		for (auto &it:roads) {
			Road &road = it.second;
			std::string roadId = road.getId(); //road id
			std::string junctionId = road.getJunctionId(); // road junction


			std::vector<LaneSection> &laneSections = road.getLanes().getLaneSections();
			for (auto laneSection = laneSections.begin(); laneSection != laneSections.end(); laneSection++) {
				//分别构建左侧和右侧的车道连接关系
				bool left = true;
				bool right = false;
				if (false == buildOneSideLink(roadId, laneSection - laneSections.begin(), left) || \
					false == buildOneSideLink(roadId, laneSection - laneSections.begin(), right)) {
					return false;
				}
			}
		}

		//遍历junction
		for (auto &item : junctions) {
			Junction &junction = item.second;
			std::vector<Connection>& connections = junction.getConnections();

			for (Connection& connection : connections) {
				std::string incomingRoad = connection.getIncomingRoad();
				std::string connectingRoad = connection.getConnectionRoad();
				std::string contactPoint = connection.getContactPoint();
				std::vector<LaneLink> &laneLinks = connection.getLaneLinks();

				for (LaneLink& laneLink : laneLinks) {
					std::string from = laneLink.getFrom();
					std::string to = laneLink.getTo();
					if (false == buildLaneLinkBetweenTwoRoad(incomingRoad, connectingRoad, contactPoint, atoi(from.c_str()), atoi(to.c_str()))) {
						return false;
					}
				}
			}
		}
		return true;
	}

	//将openDrive文档中的标签和属性解析成对应的对象和属性
	bool OpenDrive::parseOpenDriveFile()
	{
		//如果无效则退出
		if (!valid()) {
			return false;
		}
		//openDrive文档第一个标签是<OpenDRIVE>
		TiXmlElement *OpenDRIVE = doc->FirstChildElement();
		for (TiXmlElement *firstElem = OpenDRIVE->FirstChildElement(); firstElem != nullptr; firstElem = firstElem->NextSiblingElement()) {
			//解析Road
			if (0 == strcmp(firstElem->Value(), "road")) {
				std::string roadName = firstElem->Attribute("name");
				float roadLength = atof(firstElem->Attribute("length"));
				std::string roadId = firstElem->Attribute("id");
				std::string roadJunction = firstElem->Attribute("junction");
				Road road(roadName, roadLength, roadId, roadJunction);
				//vector<float> geoDis;
				for (TiXmlElement *secondElem = firstElem->FirstChildElement(); secondElem != nullptr; secondElem = secondElem->NextSiblingElement()) {
					//road间link
					if (0 == strcmp(secondElem->Value(), "link")) {
						Link link;
						for (TiXmlElement *thirdElem = secondElem->FirstChildElement(); thirdElem != nullptr; thirdElem = thirdElem->NextSiblingElement()) {
							if (0 == strcmp(thirdElem->Value(), "predecessor")) {
								std::string elementType = thirdElem->Attribute("elementType");
								std::string elementId = thirdElem->Attribute("elementId");
								std::string contactPoint = "";
								for (TiXmlAttribute* pAttr = thirdElem->FirstAttribute(); pAttr; pAttr = pAttr->Next()) {
									if (0 == strcmp(pAttr->Name(), "contactPoint")) {
										contactPoint = pAttr->Value();
									}
								}

								Predecessor* roadPre = new Predecessor(elementType, elementId, contactPoint);
								link.setPredecessor(roadPre);
							}
							if (0 == strcmp(thirdElem->Value(), "successor")) {
								std::string elementType = thirdElem->Attribute("elementType");
								std::string elementId = thirdElem->Attribute("elementId");
								std::string contactPoint = "";
								for (TiXmlAttribute* pAttr = thirdElem->FirstAttribute(); pAttr; pAttr = pAttr->Next()) {
									for (TiXmlAttribute* pAttr = thirdElem->FirstAttribute(); pAttr; pAttr = pAttr->Next()) {
										if (0 == strcmp(pAttr->Name(), "contactPoint")) {
											contactPoint = pAttr->Value();

										}
									}
								}
								Successor *roadSuccessor = new Successor(elementType, elementId, contactPoint);
								link.setSuccessor(roadSuccessor);
							}
						}
						road.setRoadLink(link);
					}

					if (0 == strcmp(secondElem->Value(), "planView")) {
						PlanView planView;
						//cout << secondElem->Value() << std::endl;
						for (TiXmlElement *thirdElem = secondElem->FirstChildElement(); thirdElem != nullptr; thirdElem = thirdElem->NextSiblingElement()) {
							TiXmlElement *fourthElem = thirdElem->FirstChildElement();
							//直线
							if (0 == strcmp(fourthElem->Value(), "line")) {
								float s = atof(thirdElem->Attribute("s"));
								float startX = atof(thirdElem->Attribute("x"));
								float startY = atof(thirdElem->Attribute("y"));
								float heading = atof(thirdElem->Attribute("hdg"));
								float length = atof(thirdElem->Attribute("length"));


								Point startPosition(startX, startY, heading, s);
								Line *line = new Line(s, startPosition, heading, length);
								//Geometry *geo = line;
								line->calPosition(stepLength);
								planView.setLine(line);
							}
							//回旋曲线
							if (0 == strcmp(fourthElem->Value(), "spiral")) {
								float dx = 0.0, dy = 0.0, t = 0.0;

								float s = atof(thirdElem->Attribute("s"));
								float startX = atof(thirdElem->Attribute("x"));
								float startY = atof(thirdElem->Attribute("y"));
								float heading = atof(thirdElem->Attribute("hdg"));
								float length = atof(thirdElem->Attribute("length"));
								float curvStart = atof(fourthElem->Attribute("curvStart"));
								float curvEnd = atof(fourthElem->Attribute("curvEnd"));

								Point startPosition(startX, startY, heading, s);
								Spiral *spiral = new Spiral(s, startPosition, heading, length, curvStart, curvEnd);
								//Geometry *geo = spiral;
								spiral->calPosition(stepLength);

								planView.setSpiral(spiral);
							}
							//圆弧
							if (0 == strcmp(fourthElem->Value(), "arc")) {
								//cout << "line" << std::endl;
								float s = atof(thirdElem->Attribute("s"));
								float startX = atof(thirdElem->Attribute("x"));
								float startY = atof(thirdElem->Attribute("y"));
								float heading = atof(thirdElem->Attribute("hdg"));
								float length = atof(thirdElem->Attribute("length"));
								float curvature = atof(fourthElem->Attribute("curvature"));

								Point startPosition(startX, startY, heading, s);
								Arc *arc = new Arc(s, startPosition, heading, length, curvature);
								//Geometry *geo = arc;
								arc->calPosition(stepLength);
								planView.setArc(arc);

							}
							//paramPoly3曲线
							if (0 == strcmp(fourthElem->Value(), "paramPoly3")) {
								float s = atof(thirdElem->Attribute("s"));
								float startX = atof(thirdElem->Attribute("x"));
								float startY = atof(thirdElem->Attribute("y"));
								float heading = atof(thirdElem->Attribute("hdg"));
								float length = atof(thirdElem->Attribute("length"));
								float au = atof(fourthElem->Attribute("aU"));
								float bu = atof(fourthElem->Attribute("bU"));
								float cu = atof(fourthElem->Attribute("cU"));
								float du = atof(fourthElem->Attribute("dU"));
								float av = atof(fourthElem->Attribute("aV"));
								float bv = atof(fourthElem->Attribute("bV"));
								float cv = atof(fourthElem->Attribute("cV"));
								float dv = atof(fourthElem->Attribute("dV"));
								const char *pRangestr = fourthElem->Attribute("pRange");
								float pRange = 1;
								//表示存在这个属性
								if (pRangestr != 0) {
									pRange = atof(pRangestr);
								}
								Point startPosition(startX, startY, heading, s);
								ParamPoly3 *paramPoly3 = new ParamPoly3(s, startPosition, heading, length, au, bu, cu, du, av, bv, cv, dv, pRange);
								Geometry *geo = paramPoly3;
								geo->calPosition(stepLength);
								planView.setParamPoly3(paramPoly3);

							}


						}
						road.setPlaneView(planView);
					}

					if (0 == strcmp(secondElem->Value(), "lanes")) {
						Lanes lanes;
						for (TiXmlElement *thirdElem = secondElem->FirstChildElement(); thirdElem != nullptr; thirdElem = thirdElem->NextSiblingElement()) {
							if (0 == strcmp(thirdElem->Value(), "laneSection")) {
								float s = atof(thirdElem->Attribute("s"));

								LaneSection laneSection(s);
								//sigleSide
								for (TiXmlAttribute* pAttr = thirdElem->FirstAttribute(); pAttr; pAttr = pAttr->Next()) {
									if (0 == strcmp(pAttr->Name(), "singleSide")) {
										laneSection.setSingleSide(pAttr->Value());
									}
								}
								//left center right
								for (TiXmlElement *fourthElem = thirdElem->FirstChildElement(); fourthElem != nullptr; fourthElem = fourthElem->NextSiblingElement()) {
									if (0 == strcmp(fourthElem->Value(), "left")) {
										LeftCenterRight left;
										parseLeftCenterRight(left, fourthElem, roadId, lanes.getLaneSections().size(), s);
										laneSection.setLeft(left);
									}
									if (0 == strcmp(fourthElem->Value(), "center")) {
										LeftCenterRight center;
										parseLeftCenterRight(center, fourthElem, roadId, lanes.getLaneSections().size(), s);
										laneSection.setCenter(center);
									}
									if (0 == strcmp(fourthElem->Value(), "right")) {
										LeftCenterRight right;
										parseLeftCenterRight(right, fourthElem, roadId, lanes.getLaneSections().size(), s);
										laneSection.setRight(right);
									}

								}
								lanes.addLaneSections(laneSection);
							}
							if (0 == strcmp(thirdElem->Value(), "laneOffset")) {
								float roadSuccessor = atof(thirdElem->Attribute("s"));
								float a = atof(thirdElem->Attribute("a"));
								float b = atof(thirdElem->Attribute("b"));
								float c = atof(thirdElem->Attribute("c"));
								float d = atof(thirdElem->Attribute("d"));
								LaneOffset laneOffset(roadSuccessor, a, b, c, d);
								lanes.addLaneOffsets(laneOffset);
							}
						}
						road.setLanes(lanes);
					}
				}
				roads.insert(std::pair<std::string, Road>(road.getId(), road));

			}

			//解析Junction
			if (0 == strcmp(firstElem->Value(), "junction")) {
				std::string id = firstElem->Attribute("id");
				std::string name = firstElem->Attribute("name");
				Junction junction(id, name);
				for (TiXmlElement *secondElem = firstElem->FirstChildElement(); secondElem != nullptr; secondElem = secondElem->NextSiblingElement()) {
					if (0 == strcmp(secondElem->Value(), "connection")) {
						std::string id = secondElem->Attribute("id");
						std::string incomingRoad = secondElem->Attribute("incomingRoad");
						std::string connectingRoad = secondElem->Attribute("connectingRoad");
						std::string contactPoint = secondElem->Attribute("contactPoint");
						Connection connection(id, incomingRoad, connectingRoad, contactPoint);

						for (TiXmlElement *thirdElem = secondElem->FirstChildElement(); thirdElem != nullptr; thirdElem = thirdElem->NextSiblingElement()) {
							if (0 == strcmp(thirdElem->Value(), "laneLink")) {
								std::string from = thirdElem->Attribute("from");
								std::string to = thirdElem->Attribute("to");
								LaneLink lk(from, to);
								connection.addLaneLinks(lk);
							}
						}
						junction.addConnections(connection);
					}
				}

				junctions.insert(std::pair<std::string, Junction>(junction.getId(), junction));
			}
		}
		return true;
	}

	void OpenDrive::parseLeftCenterRight(LeftCenterRight & LCF, TiXmlElement * lanesElem, std::string roadId, int laneSectionId, float s)
	{
		//遍历lane
		for (TiXmlElement *laneElem = lanesElem->FirstChildElement(); laneElem != nullptr; laneElem = laneElem->NextSiblingElement()) {
			int laneId = atoi(laneElem->Attribute("id"));
			std::string type = laneElem->Attribute("type");
			std::string level = laneElem->Attribute("level");
			Lane lane(roadId, laneSectionId, laneId, type, level);
			lane.setS(s);
			//遍历lane子项
			for (TiXmlElement *laneChildren = laneElem->FirstChildElement(); laneChildren != nullptr; laneChildren = laneChildren->NextSiblingElement()) {
				//车道宽度
				if (0 == strcmp(laneChildren->Value(), "width")) {
					float a = atof(laneChildren->Attribute("a"));
					float b = atof(laneChildren->Attribute("b"));
					float c = atof(laneChildren->Attribute("c"));
					float d = atof(laneChildren->Attribute("d"));
					float sOffset = atof(laneChildren->Attribute("sOffset"));
					Width width(sOffset, a, b, c, d);
					lane.addWidths(width);
				}
				//车道连接信息
				if (0 == strcmp(laneChildren->Value(), "link")) {
					Link link;
					for (TiXmlElement *seventhElem = laneChildren->FirstChildElement(); seventhElem != nullptr; seventhElem = seventhElem->NextSiblingElement()) {

						if (0 == strcmp(seventhElem->Value(), "predecessor")) {
							Predecessor *roadPre = new Predecessor();
							roadPre->setId(seventhElem->Attribute("id"));
							link.setPredecessor(roadPre);
						}
						if (0 == strcmp(seventhElem->Value(), "successor")) {
							Successor *roadSuccessor = new Successor();
							roadSuccessor->setId(seventhElem->Attribute("id"));
							link.setSuccessor(roadSuccessor);

						}
					}
					lane.setLink(link);
				}
				//roadMark信息,目前只利用了sOffset和type和laneChange信息											
				if (0 == strcmp(laneChildren->Value(), "roadMark")) {
					float sOffset = atof(laneChildren->Attribute("sOffset"));
					std::string type = laneChildren->Attribute("type");
					std::string laneChange = "none";		//如果缺少该属性，则默认不能换道，和openDrive官方文档相反
					const char *laneChangeStr = laneChildren->Attribute("laneChange");
					if (laneChangeStr != 0) {
						laneChange = laneChangeStr;
					}
					RoadMark roadMark(sOffset, type, laneChange);
					lane.addRoadMarks(roadMark);
				}

				//车速限制信息
				if (0 == strcmp(laneChildren->Value(), "speed")) {
					float max = atof(laneChildren->Attribute("max"));
					float sOffset = atof(laneChildren->Attribute("sOffset"));
					std::string unit = laneChildren->Attribute("unit");

					Speed speed(sOffset, max, unit);
					lane.addSpeeds(speed);
				}

				//斑马线信息
				if (0 == strcmp(laneChildren->Value(), "crossWalk")) {
					float sOffset = atof(laneChildren->Attribute("sOffset"));
					float length = atof(laneChildren->Attribute("length"));
					CrossWalk crossWalk(sOffset, length);
					lane.addCrossWalk(crossWalk);

				}
				//红绿灯信息
				if (0 == strcmp(laneChildren->Value(), "signal")) {
					float sOffset = atof(laneChildren->Attribute("sOffset"));
					std::string turnLight = laneChildren->Attribute("turnLight");
					Signal signal(sOffset, turnLight);
					lane.addSignal(signal);
				}
			}
			lane.setUniqueId(laneNum++);
			nodes.push_back(Node(roadId, laneSectionId, laneId));

			if (lane.getRoadMarkSize() == 0) {
				RoadMark roadMark(s, "none", "none");
				lane.addRoadMarks(roadMark);
			}
			if (lane.getSpeedSize() == 0) {
				Speed speed(s, 40, "km/h");
				lane.addSpeeds(speed);
			}
			LCF.addLanes(lane);

		}
	}

}